/*
 * File      : clock.c
 * This file is part of RT-Thread RTOS
 * COPYRIGHT (C) 2006, RT-Thread Development Team
 *
 * The license and distribution terms for this file may be
 * found in the file LICENSE in this distribution or at
 * http://openlab.rt-thread.com/license/LICENSE
 *
 * Change Logs:
 * Date           Author       Notes
 * 2011-01-13     weety        first version
 */

#include <rtthread.h>
#include "dm368.h"
#include "system_clock.h"

#define false 0
#define true  1
int dm646x_clock_init(rt_uint32_t main_clock)
{
	unsigned tmp, freq, mckr;
	int i;
	int pll_overclock = false;

	/*
	 * When the bootloader initialized the main oscillator correctly,
	 * there's no problem using the cycle counter.  But if it didn't,
	 * or when using oscillator bypass mode, we must be told the speed
	 * of the main clock.
	 */
	/*
	if (!main_clock) 
	{
		do 
		{
			tmp = at91_sys_read(AT91_CKGR_MCFR);
		} while (!(tmp & AT91_PMC_MAINRDY));
		main_clock = (tmp & AT91_PMC_MAINF) * (AT91_SLOW_CLOCK / 16);
	}
	main_clk.rate_hz = main_clock;
	*/
	/* report if PLLA is more than mildly overclocked */
	/*
	plla.rate_hz = at91_pll_rate(&plla, main_clock, at91_sys_read(AT91_CKGR_PLLAR));
	if (plla.rate_hz > 209000000)
		pll_overclock = true;
	if (pll_overclock)
		rt_kprintf("Clocks: PLLA overclocked, %ld MHz\n", plla.rate_hz / 1000000);

	at91_pllb_usbfs_clock_init(main_clock);
	*/
	/*
	 * MCK and CPU derive from one of those primary clocks.
	 * For now, assume this parentage won't change.
	 */
//	mckr = at91_sys_read(AT91_PMC_MCKR);
//	mck.parent = at91_css_to_clk(mckr & AT91_PMC_CSS);
//	freq = mck.parent->rate_hz;
//	freq /= (1 << ((mckr & AT91_PMC_PRES) >> 2));				/* prescale */
	
//	mck.rate_hz = freq / (1 << ((mckr & AT91_PMC_MDIV) >> 8));      /* mdiv */

	/* Register the PMC's standard clocks */
//	rt_list_init(&clocks);
//	for (i = 0; i < ARRAY_SIZE(standard_pmc_clocks); i++)
//		rt_list_insert_after(&clocks, &standard_pmc_clocks[i]->node);

//	rt_list_insert_after(&clocks, &pllb.node);
//	rt_list_insert_after(&clocks, &uhpck.node);
//	rt_list_insert_after(&clocks, &udpck.node);

	/* MCK and CPU clock are "always on" */
	//clk_enable(&mck);

//	rt_kprintf("Clocks: CPU %u MHz, master %u MHz, main %u.%03u MHz\n",
//		freq / 1000000, (unsigned) mck.rate_hz / 1000000,
//		(unsigned) main_clock / 1000000,
//		((unsigned) main_clock % 1000000) / 1000);

	return 0;
}

void _wait(rt_uint32_t delay)
{
    volatile rt_uint32_t i;
    for (i = 0 ; i < delay ; i++)
		;
}

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  _enablePll0( clock_mode, multiplier, post_div )                         *
 *      Setup PLL0                                                          *
 *                                                                          *
 *      Inputs:                                                             *
 *          clock_mode -> [0:OSCIN / 1:CLKIN]                               *
 *          multiplier -> [0-63] -> 27MHz * multiplier + 1                  *
 *                                                                          *
 * ------------------------------------------------------------------------ */
rt_uint16_t dm6467_pll1_enable( rt_uint16_t clock_mode, rt_uint16_t multiplier, rt_uint16_t post_div )
{
    /*
     *  Step 0 - Ignore request if the PLL is already set as is
     *//*
    if ( ( ( *pll_ctl & 0x0100 ) >> 8 ) == clock_mode )
        if ( ( *pll_pllm & 0x3f ) == ( multiplier & 0x3f ) )
            if ( ( *pll_postdiv & 0x1f ) == ( postdiv & 0x1f ) )
                return 0;   // Already Set*/

    /*
     *  Step 0 - Stop all peripheral operations ( except DDR )
     */
rt_kprintf("=======================================PLL1_PLLCTL = 0x%x\n", PLL1_PLLCTL);
    /*
     *  Step 1 - Set clock mode
     */
    if ( clock_mode == OSCIN )
        PLL1_PLLCTL &= ~0x010;    // Onchip Oscillator

    if ( clock_mode == CLKIN )
        PLL1_PLLCTL |= 0x0100;     // External Clock

	rt_kprintf("=======================================PLL1_PLLCTL = 0x%x\n", PLL1_PLLCTL);
    /*
     *  Step 2 - Set PLL to bypass
     *         - Wait for PLL to stabilize
     */
    PLL1_PLLCTL &= ~0x0020;
    PLL1_PLLCTL &= ~0x0001;
    _wait( 150 );

    /*
     *  Step 3 - Reset PLL
     */
    PLL1_PLLCTL |= 0x0008;

    /*
     *  Step 4 - Disable PLL
     *  Step 5 - Powerup PLL
     *  Step 6 - Enable PLL
     *  Step 7 - Wait for PLL to stabilize
     */
    PLL1_PLLCTL |= 0x0010;         // Disable PLL
    PLL1_PLLCTL &= ~0x0002;        // Power up PLL
    PLL1_PLLCTL &= ~0x0010;        // Enable PLL
    _wait( 150 );               // Wait for PLL to stabilize

PLL1_PREDIV |= 0x8000;
    /*
     *  Step 8 - Load PLL multiplier
     */
    PLL1_PLLM = multiplier & 0x3f;

    /*
     *  Step 9 - Set PLL post dividers
     *           Div1: (Fixed @ /1)DSP
     *           Div2: (Fixed @ /2)ARM/PCI/GraphicsEngine/HD-VICP0-1/EDMA/SCR
     *           Div3: (Fixed @ /4)Peripherals
     *           Div4: ATA
     *           Div5: SYSCLK5
     *           Div6: SYSCLK6
     *           Div7: SYSCLK7
     *           Div8: SYSCLK8
     *           Div9: VLYNQ
     */
    PLL1_PLLDIV1 = 0x8000 | DIV_1;       // DSP
    PLL1_PLLDIV2 = 0x8000 | DIV_2;       // ARM/PCI/HDVICP
    PLL1_PLLDIV3 = 0x8000 | DIV_4;       // Peripherals
    PLL1_PLLDIV4 = 0x8000 | DIV_6;       // ATA divider
  //*pll_div5   = 0x8000 | DIV_8;
  //*pll_div6   = 0x8000 | DIV_8;
  //*pll_div7   = 0x8000 | DIV_8;
    PLL1_PLLDIV8 = 0x8000 | DIV_8;       // Video Clock
    PLL1_PLLDIV9 = 0x8000 | DIV_6;       // VLYNQ divider
  //*pll_bpdiv  = 0x0000 | bypass_div;  // Bypass divider
  //*pll_postdiv= 0x8000 | postdiv;     // Post divider
  PLL1_POSTDIV |= 0x8000;//fify
    PLL1_PLLCMD |= 0x0001;                 // GO
    _wait( 2000 );

    /*
     *  Step 10 - Wait for PLL to reset ( 2000 cycles )
     *  Step 11 - Release from reset
     */
    PLL1_PLLCTL &= ~0x0008;
    _wait( 2000 );

    /*
     *  Step 12 - Wait for PLL to re-lock ( 2000 cycles )
     *  Step 13 - Switch out of BYPASS mode
     */
    PLL1_PLLCTL |= 0x0001;
    _wait( 2000 );

    return 0;
}

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  _enablePll1( clock_mode, multiplier, ddr2_div )                         *
 *      Setup PLL1                                                          *
 *                                                                          *
 *      Inputs:                                                             *
 *          clock_mode -> [0:OSCIN / 1:CLKIN]                               *
 *          multiplier -> [0-63] -> 27MHz * multiplier + 1                  *
 *          ddr2_div   -> [0-31] -> 27MHz * multiplier + 1 / 2 * ddr2_div   *
 *                                                                          *
 * ------------------------------------------------------------------------ */
rt_uint16_t dm6467_pll2_enable( rt_uint16_t clock_mode, rt_uint16_t multiplier, rt_uint16_t ddr2_div )
{
//    rt_uint32_t evmdm6467_enablepll1_fn;

    volatile rt_uint32_t* pll_ctl    = ( volatile rt_uint32_t* )&PLL2_PLLCTL;
    volatile rt_uint32_t* pll_pllm   = ( volatile rt_uint32_t* )&PLL2_PLLM;
    volatile rt_uint32_t* pll_cmd    = ( volatile rt_uint32_t* )&PLL2_PLLCMD;
    volatile rt_uint32_t* pll_stat   = ( volatile rt_uint32_t* )&PLL2_PLLSTAT;
    volatile rt_uint32_t* pll_div1   = ( volatile rt_uint32_t* )&PLL2_PLLDIV1;


//    evmdm6467_enablepll1_fn = ( rt_uint32_t )&EVMDM6467_enablePll1;
//    if ( ( evmdm6467_enablepll1_fn >= ( DDR_BASE ) )
//      && ( evmdm6467_enablepll1_fn <= ( DDR_BASE + DDR_SIZE ) ) )
//        return -1;

    /*
     *  Step 0 - Ignore request if the PLL is already set as is
     *//*
    if ( ( ( *pll_ctl & 0x0100 ) >> 8 ) == clock_mode )
        if ( ( *pll_pllm & 0x3f ) == ( multiplier & 0x3f ) )
            if ( ( *pll_div1 & 0x1f ) == ( ddr2_div & 0x1f ) )
            return 0;*/

    /*
     *  Step 1 - Set clock mode
     */
    if ( clock_mode == OSCIN )
        *pll_ctl &= ~0x0100;    // Onchip Oscillator

    if ( clock_mode == CLKIN )
        *pll_ctl |= 0x0100;     // External Clock

    /*
     *  Step 2 - Set PLL to bypass
     *         - Wait for PLL to stabilize
     */
    *pll_ctl &= ~0x0020;
    *pll_ctl &= ~0x0001;
    _wait( 150 );

    /*
     *  Step 3 - Reset PLL
     */
    *pll_ctl |= 0x0008;

    /*
     *  Step 4 - Disable PLL
     *  Step 5 - Powerup PLL
     *  Step 6 - Enable PLL
     *  Step 7 - Wait for PLL to stabilize
     */
    *pll_ctl |= 0x0010;         // Disable PLL
    *pll_ctl &= ~0x0002;        // Power up PLL
    *pll_ctl &= ~0x0010;        // Enable PLL
    _wait( 150 );               // Wait for PLL to stabilize

    /*
     *  Step 8 - Load PLL multiplier
     */
    *pll_pllm = multiplier & 0x3f;

    /*
     *  Step 9 - Load PLL dividers ( 1:DDR2 )
     */
    *pll_div1 = 0x8000 | ( ddr2_div & 0x1f );
    *pll_cmd |= 0x0001;             // GO
    while( ( *pll_stat & 1 ) != 0 );// Wait for phase alignment
    _wait( 2000 );

    /*
     *  Step 10 - Wait for PLL to reset ( 2000 cycles )
     *  Step 11 - Release from reset
     */
    *pll_ctl &= ~0x0008;
    _wait( 2000 );

    /*
     *  Step 12 - Wait for PLL to re-lock ( 2000 cycles )
     *  Step 13 - Switch out of BYPASS mode
     */
    *pll_ctl |= 0x0001;
    _wait( 2000 );

    return 0;
}

static inline unsigned pll_prediv(volatile void *pllbase)
{
	return 1;
}

static inline unsigned pll_postdiv(volatile void *pllbase)
{
	return 1;
}

static rt_uint32_t davinci_sysclk_get(rt_uint8_t sysclk)
{
	rt_uint32_t base = CLK_IN / 1000;

	if (PLL1_PLLCTL & 0x1) //if PLL ON
	{
		rt_kprintf("PLL1_PREDIV = 0x%x PLL1_PLLM = 0x%x PLL1_POSTDIV = 0x%x\n", PLL1_PREDIV, PLL1_PLLM, PLL1_POSTDIV);
		rt_kprintf("PLL2_PREDIV = 0x%x PLL2_PLLM = 0x%x PLL2_POSTDIV = 0x%x\n", PLL2_PREDIV, PLL2_PLLM, PLL2_POSTDIV);
		rt_kprintf("PLL1_PLLDIV8 = 0x%x PLL1_PLLDIV4 = 0x%x\n", PLL1_PLLDIV8, PLL1_PLLDIV4);
		rt_kprintf("PLL2_PLLDIV2 = 0x%x\n", PLL2_PLLDIV2);

		base /= pll_prediv((volatile void *)DM368_BASE_PLL1);
		/// 	base *= 1 + (PLL1_PLLM & 0x0ff);
		base *= 2 * (PLL1_PLLM & 0x0ff);
		base /= pll_postdiv((volatile void *)DM368_BASE_PLL1);	
	}

	switch(sysclk)
	{
		case SYSCLK1:
		if (PLL1_PLLDIV1 & (1<<15)) //if Divider enable for SYSCLK1
		{
			base /= ((PLL1_PLLDIV1&0xf) + 1);
		}
		break;
		case SYSCLK2:
		if (PLL1_PLLDIV2 & (1<<15)) //if Divider enable for SYSCLK2
		{
			base /= ((PLL1_PLLDIV2&0xf) + 1);
		}			
		break;
		case SYSCLK3:
		if (PLL1_PLLDIV3 & (1<<15)) //if Divider enable for SYSCLK3
		{
			base /= ((PLL1_PLLDIV3&0xf) + 1);
		}		
		break;		
		case SYSCLK4:
		if (PLL1_PLLDIV4 & (1<<15)) //if Divider enable for SYSCLK4
		{
			base /= ((PLL1_PLLDIV4&0xf) + 1);
		}		
		break;		
		case SYSCLK5:
		if (PLL1_PLLDIV5& (1<<15)) //if Divider enable for SYSCLK5
		{
			base /= ((PLL1_PLLDIV5&0xf) + 1);
		}		
		break;
		case SYSCLK6:
		if (PLL1_PLLDIV6 & (1<<15)) //if Divider enable for SYSCLK6
		{
			base /= ((PLL1_PLLDIV6&0xf) + 1);
		}		
		break;
		case SYSCLK8:
		if (PLL1_PLLDIV8 & (1<<15)) //if Divider enable for SYSCLK8
		{
			base /= ((PLL1_PLLDIV8&0xf) + 1);
		}		
		break;		
	}
	
	return (base * 1000);

/*
	rt_uint32_t base = CLK_IN / 1000;

	if (readl(PLL1_PLLCTL) & 0x1) //if PLL ON
	{
		base /= pll_prediv((volatile void *)DM646X_BASE_PLL1);
		base *= 1 + (readl(PLL1_PLLM)& 0x0ff);
		base /= pll_postdiv((volatile void *)DM646X_BASE_PLL1);	
	}

	switch(sysclk)
	{
		case SYSCLK1:
		if (readl(PLL1_PLLDIV1) & (1<<15)) //if Divider enable for SYSCLK1
		{
			base /= ((readl(PLL1_PLLDIV1)&0xf) + 1);
		}
		break;
		case SYSCLK2:
		if (readl(PLL1_PLLDIV2) & (1<<15)) //if Divider enable for SYSCLK2
		{
			base /= ((readl(PLL1_PLLDIV2)&0xf) + 1);
		}			
		break;
		case SYSCLK3:
		if (readl(PLL1_PLLDIV3) & (1<<15)) //if Divider enable for SYSCLK3
		{
			base /= ((readl(PLL1_PLLDIV3)&0xf) + 1);
		}		
		break;		
		case SYSCLK4:
		if (readl(PLL1_PLLDIV4) & (1<<15)) //if Divider enable for SYSCLK4
		{
			base /= ((readl(PLL1_PLLDIV4)&0xf) + 1);
		}		
		break;		
		case SYSCLK5:
		if (readl(PLL1_PLLDIV5) & (1<<15)) //if Divider enable for SYSCLK5
		{
			base /= ((readl(PLL1_PLLDIV5)&0xf) + 1);
		}		
		break;
		case SYSCLK6:
		if (readl(PLL1_PLLDIV6) & (1<<15)) //if Divider enable for SYSCLK6
		{
			base /= ((readl(PLL1_PLLDIV6)&0xf) + 1);
		}		
		break;
		case SYSCLK8:
		if (readl(PLL1_PLLDIV8) & (1<<15)) //if Divider enable for SYSCLK8
		{
			base /= ((readl(PLL1_PLLDIV8)&0xf) + 1);
		}		
		break;		
	}
	
	return (base * 1000);
*/	
}

/**
 * davinci_set_pllrate - set the output rate of a given PLL.
 *
 * Note: Currently tested to work with OMAP-L138 only.
 *
 * @pll: pll whose rate needs to be changed.
 * @prediv: The pre divider value. Passing 0 disables the pre-divider.
 * @pllm: The multiplier value. Passing 0 leads to multiply-by-one.
 * @postdiv: The post divider value. Passing 0 disables the post-divider.
 */
int davinci_set_pllrate(unsigned int prediv, unsigned int mult, unsigned int postdiv)
{
	unsigned int ctrl;
	unsigned int locktime;

	/*
	 *  PLL lock time required per OMAP-L138 datasheet is
	 * (2000 * prediv)/sqrt(pllm) OSCIN cycles. We approximate sqrt(pllm)
	 * as 4 and OSCIN cycle as 25 MHz.
	 */
	if (prediv) 
	{
		locktime = ((2000 * prediv) / 100);
		prediv = (prediv - 1) | (1<<15);
	} 
	else 
	{
		locktime = 20;
	}
	if (postdiv)
		postdiv = (postdiv - 1) | (1<<15);
	if (mult)
		mult = mult - 1;

	ctrl = PLL1_PLLCTL;

	/* PLLPWRDN(PLLCTL bit 1) = 0 power up the PLL */
	ctrl &= ~(1<<1);
	PLL1_PLLCTL = ctrl;

	ctrl &= ~(1<<5);
	PLL1_PLLCTL = ctrl;

	ctrl &= ~(1<<1);
	PLL1_PLLCTL = ctrl;	

//	udelay(PLL_BYPASS_TIME);
    _wait( 150 );

	/* Reset and enable PLL */
	ctrl |= (1<<3);
	PLL1_PLLCTL = ctrl;

    _wait( 500 );

	ctrl &= ~(1<<3);
	PLL1_PLLCTL = ctrl;
	
	PLL1_PREDIV = prediv;

	PLL1_PLLM = mult;

	PLL1_POSTDIV = postdiv;

	PLL1_PLLSECCTL |= 1<<17; //TENABLE = 1
	PLL1_PLLSECCTL |= 1<<18; //TENABLEDIV = 1
	PLL1_PLLSECCTL |= 1<<16; //TINITZ = 1

	PLL1_PLLSECCTL |= 1<<17; //TENABLE = 1
	PLL1_PLLSECCTL |= 1<<18; //TENABLEDIV = 1
	PLL1_PLLSECCTL &= ~(1<<16); //TINITZ = 0

	PLL1_PLLSECCTL &= ~(1<<17); //TENABLE = 0
	PLL1_PLLSECCTL &= ~(1<<18); //TENABLEDIV = 0
	PLL1_PLLSECCTL &= ~(1<<16); //TINITZ = 0

	PLL1_PLLSECCTL &= ~(1<<17); //TENABLE = 0
	PLL1_PLLSECCTL &= ~(1<<18); //TENABLEDIV = 0
	PLL1_PLLSECCTL |= 1<<16; //TINITZ = 1	
	
	  PLL1_PLLDIV1 = 0x8000 | DIV_1;	   // DSP
	  PLL1_PLLDIV2 = 0x8000 | DIV_2;	   // ARM/PCI/HDVICP
	  PLL1_PLLDIV3 = 0x8000 | DIV_4;	   // Peripherals
	  PLL1_PLLDIV4 = 0x8000 | DIV_6;	   // ATA divider
	//*pll_div5   = 0x8000 | DIV_8;
	//*pll_div6   = 0x8000 | DIV_8;
	//*pll_div7   = 0x8000 | DIV_8;
	  PLL1_PLLDIV8 = 0x8000 | DIV_8;	   // Video Clock
	  PLL1_PLLDIV9 = 0x8000 | DIV_6;	   // VLYNQ divider
	//*pll_bpdiv  = 0x0000 | bypass_div;  // Bypass divider
	//*pll_postdiv= 0x8000 | postdiv;	  // Post divider
	PLL1_POSTDIV |= 0x8000;//fify
	  PLL1_PLLCMD |= 0x0001;				 // GO
	  _wait( 2000 );
	
	  /*
	   *  Step 10 - Wait for PLL to reset ( 2000 cycles )
	   *  Step 11 - Release from reset
	   */
	  PLL1_PLLCTL &= ~0x0008;
	  _wait( 2000 );
	
	  /*
	   *  Step 12 - Wait for PLL to re-lock ( 2000 cycles )
	   *  Step 13 - Switch out of BYPASS mode
	   */
	  PLL1_PLLCTL |= 0x0001;
	  _wait( 2000 );
	
	  return 0;

}

/**
 * @brief System Clock Configuration
 */
void rt_hw_clock_init(void)
{
	dm646x_clock_init(18432000);
//	dm6467_pll1_enable(OSCIN, 10, 0);
///	davinci_set_pllrate(1, 10, 1);
//	dm6467_pll2_enable(OSCIN, 21, 0);
rt_kprintf("ARM %u MHz\n", davinci_arm_clk_get()/1000000);
rt_kprintf("MMC %u MHz\n", davinci_sysclk_get(SYSCLK8)/1000000);
}

rt_uint32_t davinci_arm_clk_get(void)
{
	return davinci_sysclk_get(SYSCLK2);
/*
	rt_uint32_t base = CFG_REFCLK_FREQ / 1000;

	if (readl(PLL1_PLLCTL) & 0x1) //if PLL ON
	{
		base /= pll_prediv((volatile void *)DM646X_BASE_PLL1);
		base *= 1 + (readl(PLL1_PLLM)& 0x0ff);
		base /= pll_postdiv((volatile void *)DM646X_BASE_PLL1);	
	}

	if (readl(PLL1_PLLDIV2) & (1<<15)) //if Divider enable for SYSCLK2
	{
		base /= ((readl(PLL1_PLLDIV2)&0xf) + 1);
	}
	
	return (base * 1000);
*/
}

rt_uint32_t davinci_timer0_clk_get(void)
{
//	return davinci_sysclk_get(SYSCLK3);
    return 24000000;
}

rt_uint32_t davinci_emac_get(void)
{
	return davinci_sysclk_get(SYSCLK3);
}

